package nape.constraint;
import nape.phys.PhysObj;
import nape.space.Space;
import nape.geom.VecMath;
import nape.geom.Vec2;
import nape.constraint.ClassicCons;
import nape.constraint.Constraint;
import nape.Const;
import nape.util.FastMath;
















class SlideJoint extends ClassicCons {

	vec_new(public,n);
	
	public var jointMin:Float;
	public var jointMax:Float;
	
	public var nMass:Float;
	public var bias :Float;
	public var jnAcc:Float;
	public var jnMax:Float;
	
	public var slack:Bool;
	public var scale:Float;
	
	
	
	public function new (obj1:PhysObj,obj2:PhysObj,anchor1:Vec2,anchor2:Vec2,min:Float,max:Float) {
		super(obj1,obj2,anchor1,anchor2);
		
		vec_new(sp); vec_sub(anchor1.p,anchor2.p,sp);
		jointMin = min;
		jointMax = max;
		jnAcc = 0;
	}
	
	
	
	public override function preStep(dt:Float) {
		ranchor(a1,b1,r1);
		ranchor(a2,b2,r2);
		
		rdelta(b1,b2,r1,r2,n);
		var dist; delta_norm(n,dist);
		
		if(dist>jointMax)  {
			dist = jointMax-dist;
			scale = 1;
			slack = false;
		}else if(dist<jointMin) {
			dist = dist-jointMin;
			scale = -1;
			slack = false;
		}else {
			dist  = 0;
			jnAcc = 0;
			slack = true;
		}
		
		if(!slack) {
			nMass = 1.0 / KScalar(b1,b2,n);
			bias = biasCoef*dist/dt;
			bias = Calc.clamp(bias,maxBias);
			jnMax = maxForce*dt;
		
			AppImp(b1,b2,r1,r2,n,jnAcc*scale,0,);
		}
		
		return false;
	}
	
	
	
	public override function applyImpulse() {
		if(slack) return false;
		
		vec_new(vr); calc_relvel(b1,b2,r1,r2,vr,);
		var jn = nMass*(bias-vec_dot(vr,n)*scale);
		var jnOld = jnAcc; jnAcc += jn;
		
		if(breakable) { if(jnAcc*jnAcc>jnMax*jnMax) return true; }
		jnAcc = Calc.range(jnAcc,-jnMax,0);
		
		jn = (jnAcc-jnOld)*scale;
		AppImp(b1,b2,r1,r2,n,jn,0,);
		
		return false;
	}
	
	
	
	public override function impulse(obj:PhysObj,dst:Vec2) {
		if(slack) vec_set(dst.p,0,0);
		else {
			if(obj==b1) vec_mul(n,-jnAcc*scale,dst.p);
			else        vec_mul(n, jnAcc*scale,dst.p);
		}
	}
}